#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/image.hpp>
#include <opencv2/opencv.hpp>
#include <chrono>
#include <iomanip>
#include <sstream>
#include <filesystem>  // C++17文件系统库

using namespace std::chrono_literals;
namespace fs = std::filesystem;  // 命名空间别名

class ImageSubscriber : public rclcpp::Node
{
public:
    ImageSubscriber() : Node("image_subscriber"), last_save_time_(this->now())
    {
        subscription_ = this->create_subscription<sensor_msgs::msg::Image>(
            "image_topic", 10, std::bind(&ImageSubscriber::image_callback, this, std::placeholders::_1));
        
        // 确保保存目录存在
        save_dir_ = "saved_images";
        try {
            if (!fs::exists(save_dir_)) {
                if (fs::create_directory(save_dir_)) {
                    RCLCPP_INFO(this->get_logger(), "已创建目录: %s", save_dir_.c_str());
                } else {
                    RCLCPP_ERROR(this->get_logger(), "无法创建目录: %s", save_dir_.c_str());
                }
            } else if (!fs::is_directory(save_dir_)) {
                RCLCPP_ERROR(this->get_logger(), "%s 已存在但不是目录", save_dir_.c_str());
            }
        } catch (const fs::filesystem_error& e) {
            RCLCPP_ERROR(this->get_logger(), "文件系统错误: %s", e.what());
        }
        
        RCLCPP_INFO(this->get_logger(), "图像订阅者已启动，图像将保存到: %s", save_dir_.c_str());
    }

private:
    void image_callback(const sensor_msgs::msg::Image::SharedPtr msg)
    {
        // 检查是否达到3秒保存间隔
        auto now = this->now();
        if ((now - last_save_time_).seconds() < 3.0) {
            return;
        }
        last_save_time_ = now;
        
        try {
            // 将ROS图像消息转换为OpenCV格式
            cv::Mat image(
                msg->height, msg->width, 
                msg->encoding == "bgr8" ? CV_8UC3 : CV_8UC1,
                const_cast<unsigned char*>(msg->data.data()), msg->step);
            
            // 如果是灰度图像，转换为彩色以便保存
            if (msg->encoding != "bgr8") {
                cv::cvtColor(image, image, cv::COLOR_GRAY2BGR);
            }
            
            // 生成带时间戳的文件名
            auto now = std::chrono::system_clock::now();
            auto now_c = std::chrono::system_clock::to_time_t(now);
            std::stringstream ss;
            ss << save_dir_ << "/image_" 
               << std::put_time(std::localtime(&now_c), "%Y%m%d_%H%M%S") 
               << ".png";
            
            // 保存图像
            if (cv::imwrite(ss.str(), image)) {
                RCLCPP_INFO(this->get_logger(), "图像已保存: %s", ss.str().c_str());
            } else {
                RCLCPP_ERROR(this->get_logger(), "无法保存图像: %s", ss.str().c_str());
            }
        } catch (const cv::Exception& e) {
            RCLCPP_ERROR(this->get_logger(), "OpenCV处理错误: %s", e.what());
        }
    }
    
    rclcpp::Subscription<sensor_msgs::msg::Image>::SharedPtr subscription_;
    rclcpp::Time last_save_time_;
    std::string save_dir_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImageSubscriber>());
    rclcpp::shutdown();
    return 0;
}